#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S3,     MUX,            sensorI2CCustomFastSkipStates)
#pragma config(Sensor, S4,     ProtoBoard,     sensorI2CCustomFastSkipStates9V)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     RightBack,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     LeftBack,      tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     RightFront,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     LeftFront,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     Lift1,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     Lift2,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_1,     Lift3,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_2,     Lift4,         tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    LeftGripper1,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    RightGripper1,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    LeftGripper2,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    RightGripper2,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    Forward,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_6,    Side,                 tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

#include "SmuxConfig.h"
#include "drivers/hitechnic-eopd.h"

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// Prior to the start of tele-op mode, you may want t o perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
////////////////////////////////////////////////////////////////////////////////////////////////////

#define rightjoy joystick.joy1_y1/127.0*100.0
#define leftjoy joystick.joy1_y2/127.0*100.0
#define leftjoy2 -joystick.joy2_y2
#define rightjoy2 -joystick.joy2_y1

typedef struct
{
	bool pressed;
	bool open;
} servobuttons;

servobuttons frontGripper;
servobuttons backGripper;
bool slow=false;

void frontGripperAdjust(int position)
{
	servo[LeftGripper1]=255-position;
	servo[RightGripper1]=position;
}
void backGripperAdjust(int position)
{
	servo[LeftGripper2]=255-position;
	servo[RightGripper2]=position;
}

void initializeRobot()
{
	servoChangeRate[LeftGripper1]=5;
	servoChangeRate[LeftGripper2]=5;
	servoChangeRate[RightGripper1]=5;
	servoChangeRate[RightGripper2]=5;
	// Place code here to sinitialize servos to starting positions.
	// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
	frontGripper.open=false;
	backGripper.open=false;
	frontGripperAdjust(128);
	backGripperAdjust(128);
	return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////


task main()
{
	servobuttons gripper;

	HTEOPDsetShortRange(EOPD);

	initializeRobot();

	waitForStart();   // wait for start of tele-op phase

	while (true)
	{

		if(HTEOPDreadRaw(EOPD)>10)
		{
			PlayTone(2500,1);
		}
	//writeDebugStreamLine("%i",HTEOPDreadRaw(EOPD));
		///////////////////////////////////////////////////////////
		///////////////////////////////////////////////////////////
		////                                                   ////
		////      Add your robot specific tele-op code here.   ////
		////                                                   ////
		///////////////////////////////////////////////////////////
		///////////////////////////////////////////////////////////
		bool flipped=false;

		getJoystickSettings(joystick);

		if(joy1Btn(7)&&!flipped)
		{
			flipped=true;
			slow=!slow;
		}
		if(!joy1Btn(7))
		{
			flipped=false;
		}
		if(!slow)
		{
			if (abs(leftjoy)>10)
			{
				motor[LeftFront]=leftjoy;
				motor[LeftBack]=leftjoy;
			}
			else
			{
				motor[LeftFront]=0;
				motor[LeftBack]=0;
			}
			if (abs(rightjoy)>10)
			{
				motor[RightFront]=rightjoy;
				motor[RightBack]=rightjoy;
			}
			else
			{
				motor[RightFront]=0;
				motor[RightBack]=0;
			}
		}
		//slow
		else
		{
			if (abs(leftjoy)>10)
			{
				motor[LeftFront]=leftjoy/3;
				motor[LeftBack]=leftjoy/3;
			}
			else
			{
				motor[LeftFront]=0;
				motor[LeftBack]=0;
			}
			if (abs(rightjoy)>10)
			{
				motor[RightFront]=rightjoy/3;
				motor[RightBack]=rightjoy/3;
			}
			else
			{
				motor[RightFront]=0;
				motor[RightBack]=0;
			}
		}

		if (abs(leftjoy2)>10)
		{
			motor[Lift1]=leftjoy2;
			motor[Lift2]=leftjoy2;
			motor[Lift3]=leftjoy2;
			motor[Lift4]=leftjoy2;
		}
		else
		{
			motor[Lift1]=0;
			motor[Lift2]=0;
			motor[Lift3]=0;
			motor[Lift4]=0;
		}

		if (joystick.joy2_TopHat==4)
		{
			servo[Forward]=0;
		}
		else if(joystick.joy2_TopHat==0)
		{
			servo[Forward]=255;
		}
		else
		{
			servo[Forward]=128;
		}

		if (joystick.joy2_TopHat==6)
		{
			servo[Side]=255;
		}
		else if(joystick.joy2_TopHat==2)
		{
			servo[Side]=0;
		}
		else
		{
			servo[Side]=128;
		}

		if((joy2Btn(4))&&!frontGripper.pressed)
		{
			frontGripper.pressed=true;
			frontGripper.open=!frontGripper.open;
		}
		if(!(joy2Btn(4)))
		{
			frontGripper.pressed=false;
		}
		if((joy2Btn(2))&&!backGripper.pressed)
		{
			backGripper.pressed=true;
			backGripper.open=!backGripper.open;
		}
		if(!(joy2Btn(2)))
		{
			gripper.pressed=false;
			backGripper.pressed=false;
		}
		if(joy2Btn(1))
		{
			frontGripper.open=false;
			backGripper.open=false;
			backGripperAdjust(128);
			//wait10Msec(40);
			frontGripperAdjust(128);
		}
		if(joy2Btn(3))
		{
			frontGripper.open=true;
			backGripper.open=true;
		}
		if (!frontGripper.open)
		{
			frontGripperAdjust(128);
		}
		if (frontGripper.open)
		{
			frontGripperAdjust(25);
		}
		if (!backGripper.open)
		{
			backGripperAdjust(128);
		}

		if (backGripper.open)
		{
			backGripperAdjust(25);
		}
	}
}
